#include <iostream>
#include <stdio.h>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>
#include <geometry_msgs/PoseStamped.h>
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include "mynteye/api/api.h"
using namespace std;
using namespace cv;
MYNTEYE_USE_NAMESPACE
void find_feature_matches (
    const Mat& img_1, const Mat& img_2,
    std::vector<KeyPoint>& keypoints_1,
    std::vector<KeyPoint>& keypoints_2,
    std::vector< DMatch >& matches );

// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );
int main ( int argc, char** argv )
{
	
    // 内参
    double fx = 367.36163228063242059, fy = 365.90055204172193726, cx = 377.17845994262921749, cy = 243.29368789896827252;
    // 基线
    double b = 120.38269336895042727;
    //-- 读取图像
    Mat img_1 = imread ( "./left_1.jpg", CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

    // 建立3D点
    Mat left = imread("./left_1.jpg",0);
    Mat right = imread("./right_1.jpg",0);
    Mat K = (Mat_<double>(3,3) << fx , 0 , cx , 0 , fy , cy , 0 , 0 ,1 );
    vector<Point3f> pts_3d;
    vector<Point2f> pts_2d;
    Mat d1 = imread ( "./depth_1.png", CV_LOAD_IMAGE_UNCHANGED ); 
    for ( DMatch m:matches )
    {
        ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        if ( d == 0 )
        {
            printf("bad\n");
            continue;
        }
        float dd = d/5000.0;
        printf("dd=%f \n",dd);
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
        pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
        pts_2d.push_back ( keypoints_2[m.trainIdx].pt );  
    }
    cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;
 
    Mat r, t;
    solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    Mat R;
    cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵

    cout<<"R="<<endl<<R<<endl;
    cout<<"t="<<endl<<t<<endl;
  ros::init(argc, argv, "tf_publisher");
  ros::NodeHandle n;
  ros::Rate ri(100);
  tf::TransformBroadcaster broadcaster;
  Eigen::Matrix3d t_R;
  t_R<<R.at<double>(0,0),R.at<double>(0,1),R.at<double>(0,2),
       R.at<double>(1,0),R.at<double>(1,1),R.at<double>(1,2),
       R.at<double>(2,0),R.at<double>(2,1),R.at<double>(2,2);
  Eigen::Quaterniond Q3(t_R);
  double qx,qy,qz,qw;
  qx=Q3.x();
  qy=Q3.y();
  qz=Q3.z();
  qw=Q3.w();
  double tx,ty,tz;
  //tx=-0.0166822;
  //ty=-3.23848*0.00001;
  //tz=0.00747079;
  tx=t.at<double>(0);
  ty=t.at<double>(1);
  tz=t.at<double>(2);
  cout<<"R="<<endl<<t_R<<endl;
  cout<<"t="<<endl<<tx<<","<<ty<<","<<tz<<endl;
  while(n.ok()){
    broadcaster.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion(qx, qy, qz, qw), tf::Vector3(tx,ty,tz)),ros::Time::now(),"base_link", "base_laser"));
    ri.sleep();
}
}
void find_feature_matches ( const Mat& img_1, const Mat& img_2,
                            std::vector<KeyPoint>& keypoints_1,
                            std::vector<KeyPoint>& keypoints_2,
                            std::vector< DMatch >& matches )
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect ( img_1,keypoints_1 );
    detector->detect ( img_2,keypoints_2 );

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute ( img_1, keypoints_1, descriptors_1 );
    descriptor->compute ( img_2, keypoints_2, descriptors_2 );

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> match;
    // BFMatcher matcher ( NORM_HAMMING );
    matcher->match ( descriptors_1, descriptors_2, match );

    //-- 第四步:匹配点对筛选
    double min_dist=10000, max_dist=0;

    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        double dist = match[i].distance;
        if ( dist < min_dist ) min_dist = dist;
        if ( dist > max_dist ) max_dist = dist;
    }

    printf ( "-- Max dist : %f \n", max_dist );
    printf ( "-- Min dist : %f \n", min_dist );

    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        if ( match[i].distance <= max ( 2*min_dist, 30.0 ) )
        {
            matches.push_back ( match[i] );
        }
    }
}

Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
    return Point2d
           (
               ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
               ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
           );
}
